#include <pcl/pcl_base.h>
#include "LdPcl.h"

/**
 * @brief 构造函数，从yml文件中读入参数
 * @param const std::string cfgFile 配置文件的路径
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
PCLF::PCLF(const std::string cfgFile)
{
    cv::FileStorage LoadCfg(cfgFile, cv::FileStorage::READ);
    LoadCfg["m_NDWeight_cy"] >> m_NDWeight_cy;
    LoadCfg["m_maxIterations_cy"] >> m_maxIterations_cy;
    LoadCfg["m_distThreshold_cy"] >> m_distThreshold_cy;
    LoadCfg["m_radius1"] >> m_radius1;
    LoadCfg["m_radius2"] >> m_radius2;
    LoadCfg["m_euThreshold"] >> m_euThreshold;
    LoadCfg["m_minSize"] >> m_minSize;
    LoadCfg["m_maxSize"] >> m_maxSize;
    LoadCfg["m_NDWeight_pl"] >> m_NDWeight_pl;
    LoadCfg["m_maxIterations_pl"] >> m_maxIterations_pl;
    LoadCfg["m_distThreshold_pl"] >> m_distThreshold_pl;
    LoadCfg["m_xmin"] >> m_xmin;
    LoadCfg["m_xmax"] >> m_xmax;
    LoadCfg["m_ymin"] >> m_ymin;
    LoadCfg["m_ymax"] >> m_ymax;
    LoadCfg["m_zmin"] >> m_zmin;
    LoadCfg["m_zmax"] >> m_zmax;
    LoadCfg["m_rsearch"] >> m_rsearch;
    LoadCfg["m_minNeighborNum"] >> m_minNeighborNum;
    LoadCfg.release();
}

/**
 * @brief 点的强度相似度函数。计算两点的强度相似度。
 * 如果相似度在某个范围内则返回true，否则返回false。
 * @param const PointTypeFull& point_a      输入点a 
 * @param const PointTypeFull& point_b      输入点b 
 * @param float radiu                       半径长 
 * @return bool
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com 
 */
bool PCLF::intensitySimilarity(const PointTypeFull& point_a, const PointTypeFull& point_b, float radiu)
{
    if (abs(point_a.intensity - point_b.intensity) < 30.0f)
        return true;
    return false;
}

/**
 * @brief 点的强度与法线相似度函数。计算两点的强度与法线方向相似度。
 * 如果相似度在某个范围内则返回true，否则返回false。
 * @param const PointTypeFull& point_a      输入点a 
 * @param const PointTypeFull& point_b      输入点b 
 * @param float radiu                       半径长 
 * @return bool
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com  
 */
bool PCLF::intensityNormalSimilarity(const PointTypeFull& point_a, const PointTypeFull& point_b, float radiu)
{
    Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap(), point_b_normal = point_b.getNormalVector3fMap();
    if (std::abs(point_a.intensity - point_b.intensity) < 5.0f)
        // return true;
        if (std::abs(point_a_normal.dot(point_b_normal)) > std::cos(20.0f / 180.0f * static_cast<float>(M_PI)))
            return true;

    return false;
}

/**
 * @brief 点云滤波函数
 * @param const pcl::PointCloud<pcl::PointXYZ>::Ptr& inputCloud     输入点云 
 * @param pcl::PointCloud<pcl::PointXYZ>::Ptr& outputCloud          输出点云 
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::filter(const Pcloud& inputCloud, Pcloud& outputCloud)
{
    //半径滤波
    pcl::RadiusOutlierRemoval<Point> Point_Filter;
    Pcloud temp(new Cloud());
    // temp->points.at(0).getVector3fMap
    Point_Filter.setInputCloud(inputCloud);
    Point_Filter.setRadiusSearch(m_rsearch);
    Point_Filter.setMinNeighborsInRadius(m_minNeighborNum);
    Point_Filter.filter(*temp);
    pcl::visualization::CloudViewer V1("V1");
    V1.showCloud(temp);
    while (!V1.wasStopped())
    {
    }

    //直通滤波
    pcl::PassThrough<Point> pass;
    Pcloud Temp_Vg(new Cloud());
    Pcloud temp1(new Cloud());
    pass.setInputCloud(temp);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(m_xmin, m_xmax);
    pass.filter(*temp1);
    // pcl::visualization::CloudViewer V2("V2");
    // V2.showCloud(temp1);
    // while (!V2.wasStopped())
    // {
    // }

    Pcloud z_fil(new Cloud());
    pass.setInputCloud(temp1);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(m_ymin, m_ymax);
    pass.filter(*z_fil);
    // pcl::visualization::CloudViewer V3("V3");
    // V3.showCloud(z_fil);
    // while (!V3.wasStopped())
    // {
    // }

    pass.setInputCloud(z_fil);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(m_zmin, m_zmax);
    pass.filter(*outputCloud);
    // pcl::visualization::CloudViewer V4("V4");
    // V4.showCloud(OutputCloud);
    // while (!V4.wasStopped())
    // {
    // }

    //体素滤波
    // pcl::VoxelGrid<Point> vg;
    // vg.setInputCloud(Temp_Vg);
    // vg.setLeafSize(0.003f, 0.003f, 0.003f);
    // vg.filter(*OutputCloud);

    // pcl::visualization::CloudViewer viewer("Filter Viewer");
    // viewer.showCloud(OutputCloud);
    // while (!viewer.wasStopped())
    // {
    // }
}

/**
 * @brief 点云滤波函数
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCloud      输入点云 
 * @param pcl::PointCloud<pcl::PointXYZI>::Ptr& outputCloud           输出点云 
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::filter(const PcloudI& inputCloud, PcloudI& outputCloud)
{
    pcl::PassThrough<PointI> pass;
    PcloudI Temp_Vg(new CloudI());
    PcloudI temp1(new CloudI());
    pass.setInputCloud(inputCloud);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(m_xmin, m_xmax);
    pass.filter(*temp1);

    pcl::RadiusOutlierRemoval<PointI> Point_Filter;
    PcloudI temp(new CloudI());
    // temp->points.at(0).getVector3fMap
    Point_Filter.setInputCloud(temp1);
    Point_Filter.setRadiusSearch(m_rsearch);
    Point_Filter.setMinNeighborsInRadius(m_minNeighborNum);
    Point_Filter.filter(*temp);

    pcl::visualization::CloudViewer Viewer("Filter Viewer");
    Viewer.showCloud(temp);
    while (!Viewer.wasStopped())
    {
    }

    PcloudI z_fil(new CloudI());
    pass.setInputCloud(temp);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(m_ymin, m_ymax);
    pass.filter(*z_fil);

    pass.setInputCloud(z_fil);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(m_zmin, m_zmax);
    pass.filter(*outputCloud);

    // pcl::VoxelGrid<PointI> vg;
    // vg.setInputCloud(Temp_Vg);
    // vg.setLeafSize(0.002f, 0.002f, 0.002f);
    // vg.filter(*OutputCloud);

    // pcl::visualization::CloudViewer viewer("Filter Viewer");
    // viewer.showCloud(OutputCloud);
    // while (!viewer.wasStopped())
    // {
    // }
}

/**
 * @brief 对于堆叠Lagori的识别
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCluster      输入点云
 * @param float threshHeight                                            阈值高度
 * @return float                                                        半径值
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
float PCLF::pileLagori(const PcloudI& inputCluster, float threshHeight)
{
    pcl::PassThrough<PointI> pass;
    PcloudI temp(new CloudI());
    pass.setInputCloud(inputCluster);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(threshHeight, threshHeight + 0.005);
    pass.filter(*temp);
    pcl::visualization::CloudViewer V_Circle("Viewer Circle Fitting");
    V_Circle.showCloud(temp);
    while (!V_Circle.wasStopped())
    {
    }

    if (temp->size() < 20)
    {
        std::cout << "圆拟合点过少！！！" << std::endl;
    }
    std::vector<cv::Point2f> Points;
    float px = 0, py = 0;
    for (size_t i = 0; i < temp->points.size(); i++)
    {
        cv::Point2f tempPoint;
        tempPoint.x = temp->points[i].x;
        tempPoint.y = temp->points[i].y;
        Points.push_back(tempPoint);
        px += temp->points[i].x / temp->points.size();
        py += temp->points[i].y / temp->points.size();
    }

    //创建中间变量
    float Suuu = 0, Svvv = 0, Suu = 0, Svv = 0, Suv = 0, Suuv = 0, Suvv = 0;
    float xc, yc, r = 0;
    for (size_t i = 0; i < Points.size(); i++)
    {
        float ui = Points[i].x - px;
        float vi = Points[i].y - py;
        Suuu += pow(ui, 3);
        Svvv += pow(vi, 3);
        Suu += pow(ui, 2);
        Svv += pow(vi, 2);
        Suv += ui * vi;
        Suuv += pow(ui, 2) * vi;
        Suvv += pow(vi, 2) * ui;
    }
    float uc = (Suuu * Suv - Suuu * Svv - Suvv * Svv + Suv * Svvv) / (2 * (pow(Suv, 2) - Suu * Svv));
    float vc = (-Suu * Suuv + Suuu * Suv + Suv * Suvv - Suu * Svvv) / (2 * (pow(Suv, 2) - Suu * Svv));
    xc = uc + px;
    yc = vc + py;
    for (size_t i = 0; i < Points.size(); i++)
    {
        r += sqrt(pow(Points[i].x - xc, 2) + pow(Points[i].y - yc, 2)) / Points.size();
    }
    std::cout << "The fitting circle radiu is: " << r << std::endl;
    return r;
}

/**
 * @brief 点云法线计算
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCloud    输入点云
 * @param pcl::PointCloud<pcl::PointXYZI>::Ptr& outputNormals       输出点云
 * @param const int num                                             Ksearch的参数
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::computeNormals(const PcloudI& inputCloud, Pfcloud& outputNormals, const int num)
{
    pcl::copyPointCloud(*inputCloud, *outputNormals);
    pcl::NormalEstimation<PointI, PointTypeFull> ne;
    pcl::search::KdTree<PointI>::Ptr tree(new pcl::search::KdTree<PointI>());
    ne.setSearchMethod(tree);
    ne.setInputCloud(inputCloud);
    ne.setKSearch(num);
    ne.compute(*outputNormals);
}

/**
 * @brief 点云法线计算
 * @param const pcl::PointCloud<pcl::PointXYZ>::Ptr& inputCloud     输入点云
 * @param pcl::PointCloud<pcl::Normal>::Ptr& outputNormals          输出点云
 * @param const int num                                             Ksearch的参数
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::computeNormals(const Pcloud& inputCloud, Pcnormal& outputNormals, const int num)
{
    pcl::NormalEstimation<Point, pcl::Normal> ne;
    pcl::search::KdTree<Point>::Ptr tree(new pcl::search::KdTree<Point>());
    ne.setSearchMethod(tree);
    ne.setInputCloud(inputCloud);
    ne.setKSearch(num);
    ne.compute(*outputNormals);
}

/**
 * @brief 去除平面
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCloudPtr     输入点云
 * @param const pcl::PointCloud<pcl::Normal>::Ptr& inputNormalsPtr      输入点云的法线
 * @param int num                                                       迭代计算去除平面的次数 
 * @param bool inverted                                                 返回平面点云还是去除平面剩下的点云
 * @param pcl::PointCloud<pcl::PointXYZI>::Ptr& outputCloudPtr          输出点云
 * @param pcl::PointCloud<pcl::Normal>::Ptr& outputNormalsPtr           输出点云的法线
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::removePlanes(const PcloudI& inputCloudPtr, const Pcnormal& inputnormalsPtr, int num, bool inverted, PcloudI& outputCloudPtr,
                        Pcnormal& outputNormalsPtr)
{
    pcl::ExtractIndices<PointI> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::SACSegmentationFromNormals<PointI, Normal> seg;
    pcl::PointIndices::Ptr planes_indices(new pcl::PointIndices());
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients());
    seg.setOptimizeCoefficients(true); //设置为true为提取平面点，false则为非平面点
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
    seg.setNormalDistanceWeight(m_NDWeight_pl);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(m_maxIterations_pl);
    seg.setDistanceThreshold(m_distThreshold_pl);

    int all_points = (int)inputCloudPtr->size();
    // Pcloud temp(new Cloud());
    // *temp = *InputCloud;
    PcloudI Plane_Extration(new CloudI());
    int iteration_count = 0;
    while (iteration_count++ < num)
    {
        PcloudI temp(new CloudI());
        seg.setInputCloud(temp);
        // seg.setInputCloud(InputCloud);
        // seg.setInputNormals(normals);
        seg.segment(*planes_indices, *coefficients_plane);
        if (planes_indices->indices.size() == 0)
        {
            std::cout << "Could not estimate a planar model any more! ";
            break;
        }

        extract.setInputCloud(inputCloudPtr);
        extract.setIndices(planes_indices);
        extract.setNegative(false); //设置为false，可以将indices的点进行提取，就是反提取。
        extract.filter(*Plane_Extration);
        // pcl::visualization::CloudViewer Ex_Plane("Plane_Extraction");
        // Ex_Plane.showCloud(Plane_Extration);
        // while (!Ex_Plane.wasStopped())
        // {
        // }

        //将输入点云中的平面去除
        extract.setNegative(true); //设置为true，将平面外的点进行提取
        PcloudI Temp2(new CloudI());
        extract.filter(*Temp2);
        *outputCloudPtr = *Temp2;

        //获取提取剩余的法线
        pcl::PointCloud<pcl::Normal>::Ptr Temp_Normals(new pcl::PointCloud<pcl::Normal>());
        extract_normals.setNegative(true);
        extract_normals.setInputCloud(inputnormalsPtr);
        extract_normals.setIndices(planes_indices);
        extract_normals.filter(*Temp_Normals);
        *outputNormalsPtr = *Temp_Normals;
    }
    // pcl::visualization::CloudViewer viewer("Plane Viewer");
    // viewer.showCloud(InputCloud);
    // while (!viewer.wasStopped())
    // {
    // }
}

/**
 * @brief 去除平面
 * @param const pcl::PointCloud<pcl::PointXYZ>::Ptr& inputCloudPtr      输入点云
 * @param const pcl::PointCloud<pcl::Normal>::Ptr& inputNormalsPtr      输入点云的法线
 * @param int num                                                       迭代计算去除平面的次数 
 * @param bool inverted                                                 返回平面点云还是去除平面剩下的点云
 * @param pcl::PointCloud<pcl::PointXYZ>::Ptr& outputCloudPtr           输出点云
 * @param pcl::PointCloud<pcl::Normal>::Ptr& outputNormalsPtr           输出点云的法线
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::removePlanes(const Pcloud& inputCloudPtr, const Pcnormal& inputnormalsPtr, int num, bool inverted, Pcloud& outputCloudPtr,
                        Pcnormal& outputNormalsPtr)
{
    pcl::ExtractIndices<Point> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::SACSegmentationFromNormals<Point, Normal> seg;
    pcl::PointIndices::Ptr planes_indices(new pcl::PointIndices());
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients());
    seg.setOptimizeCoefficients(true); //设置为true为提取平面点，false则为非平面点
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
    seg.setNormalDistanceWeight(m_NDWeight_pl);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(m_maxIterations_pl);
    seg.setDistanceThreshold(m_distThreshold_pl);

    int all_points = (int)inputCloudPtr->size();
    // Pcloud temp(new Cloud());
    // *temp = *InputCloud;
    Pcloud Plane_Extration(new Cloud());
    int iteration_count = 0;
    while (iteration_count++ < num)
    {
        seg.setInputCloud(inputCloudPtr);
        seg.setInputNormals(inputnormalsPtr);
        seg.segment(*planes_indices, *coefficients_plane);
        if (planes_indices->indices.size() == 0)
        {
            std::cout << "Could not estimate a planar model any more! ";
            break;
        }

        extract.setInputCloud(inputCloudPtr);
        extract.setIndices(planes_indices);
        extract.setNegative(false); //设置为false，可以将indices的点进行提取，就是反提取。
        extract.filter(*Plane_Extration);
        // pcl::visualization::CloudViewer Ex_Plane("Plane_Extraction");
        // Ex_Plane.showCloud(Plane_Extration);
        // while (!Ex_Plane.wasStopped())
        // {
        // }

        //将输入点云中的平面去除
        extract.setNegative(true); //设置为true，将平面外的点进行提取
        Pcloud Temp2(new Cloud());
        extract.filter(*Temp2);
        *outputCloudPtr = *Temp2;

        //获取提取剩余的法线
        pcl::PointCloud<pcl::Normal>::Ptr Temp_Normals(new pcl::PointCloud<pcl::Normal>());
        extract_normals.setNegative(true);
        extract_normals.setInputCloud(inputnormalsPtr);
        extract_normals.setIndices(planes_indices);
        extract_normals.filter(*Temp_Normals);
        *outputNormalsPtr = *Temp_Normals;
    }
    // pcl::visualization::CloudViewer viewer("Plane Viewer");
    // viewer.showCloud(InputCloud);
    // while (!viewer.wasStopped())
    // {
    // }
}

/**
 * @brief 欧式聚类
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCloudPtr   输入点云
 * @param std::vector<pcl::PointIndices>& indices                     聚类结果的点云索引
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::eucluster(const PcloudI& inputCloudPtr, std::vector<pcl::PointIndices>& indices)
{
    float threshold = m_euThreshold;
    pcl::search::KdTree<PointI>::Ptr Eu_Tree(new pcl::search::KdTree<PointI>());
    pcl::EuclideanClusterExtraction<PointI> ec;
    ec.setClusterTolerance(threshold);
    ec.setMinClusterSize(m_minSize);
    ec.setMaxClusterSize(m_maxSize);
    ec.setSearchMethod(Eu_Tree);
    ec.setInputCloud(inputCloudPtr);
    ec.extract(indices);
}

/**
 * @brief 欧式聚类
 * @param const pcl::PointCloud<pcl::PointXYZ>::Ptr& inputCloudPtr   输入点云
 * @param std::vector<pcl::PointIndices>& indices                    聚类结果的点云索引
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::eucluster(const Pcloud& inputCloudPtr, std::vector<pcl::PointIndices>& indices)
{
    float threshold = m_euThreshold;
    pcl::search::KdTree<Point>::Ptr Eu_Tree(new pcl::search::KdTree<Point>());
    pcl::EuclideanClusterExtraction<Point> ec;
    ec.setClusterTolerance(threshold);
    ec.setMinClusterSize(m_minSize);
    ec.setMaxClusterSize(m_maxSize);
    ec.setSearchMethod(Eu_Tree);
    ec.setInputCloud(inputCloudPtr);
    ec.extract(indices);
}

/**
 * @brief 点云剔除器。用于根据点的索引将部分点云进行提取。
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCloudPtr   输入点云 
 * @param const pcl::PointCloud<pcl::Normal>::Ptr& inputNormalsPtr    输入点云的法线 
 * @param const pcl::PointIndices::Ptr indicesPtr                     索引
 * @param pcl::PointCloud<pcl::PointXYZI>::Ptr& outputCloudPtr        输出点云
 * @param pcl::PointCloud<pcl::Normal>::Ptr& outputNormalsPtr         输出点云的法线
 * @param bool inverted  保留要剔除的点云还是剩下的点云
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::extraction(const PcloudI& inputCloudPtr, const pcl::PointCloud<pcl::Normal>::Ptr& inputNormalsPtr, const pcl::PointIndices::Ptr indicesPtr,
                      PcloudI& outputCloudPtr, pcl::PointCloud<pcl::Normal>::Ptr& outputNormalsPtr, bool inverted)
{
    pcl::ExtractIndices<PointI> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    extract.setInputCloud(inputCloudPtr);
    extract.setIndices(indicesPtr);
    extract.setNegative(inverted);
    extract.filter(*outputCloudPtr);

    extract_normals.setNegative(inverted);
    extract_normals.setInputCloud(inputNormalsPtr);
    extract_normals.setIndices(indicesPtr);
    extract_normals.filter(*outputNormalsPtr);
}

/**
 * @brief 点云剔除器。用于根据点的索引将部分点云进行提取。
 * @param const pcl::PointCloud<pcl::PointXYZ>::Ptr& inputCloudPtr      输入点云 
 * @param const pcl::PointCloud<pcl::Normal>::Ptr& inputNormalsPtr      输入点云的法线 
 * @param const pcl::PointIndices::Ptr indicesPtr                       索引
 * @param pcl::PointCloud<pcl::PointXYZ>::Ptr& outputCloudPtr           输出点云
 * @param pcl::PointCloud<pcl::Normal>::Ptr& outputNormalsPtr           输出点云的法线
 * @param bool inverted                                                 保留要剔除的点云还是剩下的点云
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::extraction(const Pcloud& inputCloudPtr, const pcl::PointCloud<pcl::Normal>::Ptr& inputNormalsPtr, const pcl::PointIndices::Ptr indicesPtr,
                      Pcloud& outputCloudPtr, pcl::PointCloud<pcl::Normal>::Ptr& outputNormalsPtr, bool inverted)
{
    pcl::ExtractIndices<Point> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    extract.setInputCloud(inputCloudPtr);
    extract.setIndices(indicesPtr);
    extract.setNegative(inverted);
    extract.filter(*outputCloudPtr);

    extract_normals.setNegative(inverted);
    extract_normals.setInputCloud(inputNormalsPtr);
    extract_normals.setIndices(indicesPtr);
    extract_normals.filter(*outputNormalsPtr);
}

/**
 * @brief 圆柱拟合函数
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCloudPtr     输入点云
 * @param const pcl::PointCloud<pcl::Normal>::Ptr& inputNormalsPtr      输入点云的法线
 * @param pcl::PointCloud<pcl::PointXYZI>::Ptr& outputCloudPtr          输出点云
 * @param pcl::PointCloud<pcl::Normal>::Ptr& outputNormalsPtr           输出点云的法线
 * @param pcl::ModelCoefficients::Ptr& coePtr                           输出圆柱的参数 
 * @param Eigen::Vector2f& catchPoint                                   Lagori的抓取点
 * @param bool inverted                                                 保留要剔除的点云还是剩下的点云
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
bool PCLF::fitCylinder(const PcloudI& inputCloudPtr, const Pcnormal& inputNormalsPtr, PcloudI& outputCloudPtr, Pcnormal& outputNormalsPtr,
                       pcl::ModelCoefficients::Ptr& coePtr, Eigen::Vector2f& catchPoint, bool inverted)
{
    pcl::SACSegmentationFromNormals<PointI, pcl::Normal> seg;
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr Cylinder_Indices(new pcl::PointIndices());
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CYLINDER);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setNormalDistanceWeight(m_NDWeight_cy);
    seg.setMaxIterations(m_maxIterations_cy);
    seg.setDistanceThreshold(m_distThreshold_cy);
    seg.setRadiusLimits(m_radius1, m_radius2);
    seg.setInputCloud(inputCloudPtr);
    seg.setInputNormals(inputNormalsPtr);

    seg.segment(*Cylinder_Indices, *coefficients_cylinder);

    // if (Cylinder_Indices->indices.size() < 150)
    //     return false;

    if (Cylinder_Indices->indices.size() == 0)
    {
        Cylinder_Indices = nullptr;
        outputCloudPtr = nullptr;
        return false;
    }
    std::cout << "圆柱的参数为： " << *coefficients_cylinder << std::endl;
    *coePtr = *coefficients_cylinder;

    PcloudI temp(new CloudI());
    Pcnormal temp_normal(new Cnormal);
    extraction(inputCloudPtr, inputNormalsPtr, Cylinder_Indices, outputCloudPtr, outputNormalsPtr);
    extraction(inputCloudPtr, inputNormalsPtr, Cylinder_Indices, temp, temp_normal, false);
    // pcl::visualization::CloudViewer cylinder_Viewer("Cylinder_Extraction");
    // cylinder_Viewer.showCloud(temp);
    // while (!cylinder_Viewer.wasStopped())
    // {
    // }

    // pcl::visualization::CloudViewer Out_Cylinder_viewer("Out Cylinder Extraction Viewer");
    // Out_Cylinder_viewer.showCloud(OutputCloud);
    // while (!Out_Cylinder_viewer.wasStopped())
    // {
    // }
    Eigen::Vector3f CylinderVec;
    Eigen::Vector3f UnitVec;
    Eigen::Vector3f AxelPoint;
    PointI tempP1 = inputCloudPtr->points[*Cylinder_Indices->indices.begin()];
    // CylinderPoint.x() = tempP1.x;
    // CylinderPoint.y() = tempP1.y;
    // CylinderPoint.z() = tempP1.z;
    //计算从轴上点到轮廓点的向量Vec
    CylinderVec.x() = tempP1.x - coePtr->values[0];
    CylinderVec.y() = tempP1.y - coePtr->values[1];
    CylinderVec.z() = tempP1.z - coePtr->values[2];
    //获取方向向量
    UnitVec.x() = coePtr->values[3];
    UnitVec.y() = coePtr->values[4];
    UnitVec.z() = coePtr->values[5];
    //计算方向向量与Vec的点乘
    float Dot = CylinderVec.dot(UnitVec);
    AxelPoint.x() = coePtr->values[0] + UnitVec.x() * Dot;
    AxelPoint.y() = coePtr->values[1] + UnitVec.y() * Dot;
    AxelPoint.z() = coePtr->values[2] + UnitVec.z() * Dot;

    catchPoint.x() = AxelPoint.x() * 1000.;
    catchPoint.y() = AxelPoint.y() * 1000.;
    return true;
}

/**
 * @brief 圆柱拟合函数
 * @param const pcl::PointCloud<pcl::PointXYZ>::Ptr& inputCloudPtr      输入点云
 * @param const pcl::PointCloud<pcl::Normal>::Ptr& inputNormalsPtr      输入点云的法线
 * @param pcl::PointCloud<pcl::PointXYZ>::Ptr& outputCloudPtr           输出点云
 * @param pcl::PointCloud<pcl::Normal>::Ptr& outputNormalsPtr           输出点云的法线
 * @param pcl::ModelCoefficients::Ptr& coePtr                           输出圆柱的参数 
 * @param Eigen::Vector2f& catchPoint                                   Lagori的抓取点
 * @param bool inverted                                                 保留要剔除的点云还是剩下的点云
 * @author Milo
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
bool PCLF::fitCylinder(const Pcloud& inputCloudPtr, const Pcnormal& inputNormalsPtr, Pcloud& outputCloudPtr, Pcnormal& outputNormalsPtr,
                       pcl::ModelCoefficients::Ptr& coePtr, Eigen::Vector2f& catchPoint, bool inverted)
{
    pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg;
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr Cylinder_Indices(new pcl::PointIndices());
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CYLINDER);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setNormalDistanceWeight(m_NDWeight_cy);
    seg.setMaxIterations(m_maxIterations_cy);
    seg.setDistanceThreshold(m_distThreshold_cy);
    seg.setRadiusLimits(m_radius1, m_radius2);
    seg.setInputCloud(inputCloudPtr);
    seg.setInputNormals(inputNormalsPtr);

    seg.segment(*Cylinder_Indices, *coefficients_cylinder);

    // if (Cylinder_Indices->indices.size() < 150)
    //     return false;

    if (Cylinder_Indices->indices.size() == 0)
    {
        Cylinder_Indices = nullptr;
        outputCloudPtr = nullptr;
        return false;
    }
    std::cout << "圆柱的参数为： " << *coefficients_cylinder << std::endl;
    *coePtr = *coefficients_cylinder;

    Pcloud temp(new Cloud());
    Pcnormal temp_normal(new Cnormal);
    extraction(inputCloudPtr, inputNormalsPtr, Cylinder_Indices, outputCloudPtr, outputNormalsPtr);
    extraction(inputCloudPtr, inputNormalsPtr, Cylinder_Indices, temp, temp_normal, false);
    // pcl::visualization::CloudViewer cylinder_Viewer("Cylinder_Extraction");
    // cylinder_Viewer.showCloud(temp);
    // while (!cylinder_Viewer.wasStopped())
    // {
    // }

    Eigen::Vector3f CylinderVec;
    Eigen::Vector3f UnitVec;
    Eigen::Vector3f AxelPoint;
    Point tempP1 = inputCloudPtr->points[*(Cylinder_Indices->indices.begin())];
    // CylinderPoint.x() = tempP1.x;
    // CylinderPoint.y() = tempP1.y;
    // CylinderPoint.z() = tempP1.z;
    //计算从轴上点到轮廓点的向量Vec
    CylinderVec.x() = tempP1.x - coePtr->values[0];
    CylinderVec.y() = tempP1.y - coePtr->values[1];
    CylinderVec.z() = tempP1.z - coePtr->values[2];
    //获取方向向量
    UnitVec.x() = coePtr->values[3];
    UnitVec.y() = coePtr->values[4];
    UnitVec.z() = coePtr->values[5];
    //计算方向向量与Vec的点乘
    float Dot = CylinderVec.dot(UnitVec);
    AxelPoint.x() = coePtr->values[0] + UnitVec.x() * Dot;
    AxelPoint.y() = coePtr->values[1] + UnitVec.y() * Dot;
    AxelPoint.z() = coePtr->values[2] + UnitVec.z() * Dot;

    catchPoint.x() = AxelPoint.x() * 1000.;
    catchPoint.y() = AxelPoint.y() * 1000.;

    pcl::visualization::CloudViewer Out_Cylinder_viewer("Out Cylinder Extraction Viewer");
    Out_Cylinder_viewer.showCloud(outputCloudPtr);
    while (!Out_Cylinder_viewer.wasStopped())
    {
    }
    return true;
}

/**
 * @brief 画出圆柱的轴线
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCloudPtr       输入点云
 * @param const pcl::ModelCoefficients::ptr& coePtr                       圆柱的参数
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::axialDrawing(PcloudI& inputCloudPtr, const pcl::ModelCoefficients::Ptr& coePtr)
{
    PointI p1, p2;
    PointI v;
    p1.intensity = 255;
    p2.intensity = 255;

    p1.x = coePtr->values[0] * 1000.0;
    p1.y = coePtr->values[1] * 1000.0;
    p1.z = coePtr->values[2] * 1000.0;

    v.x = coePtr->values[3];
    v.y = coePtr->values[4];
    v.z = coePtr->values[5];

    float dx, dy, dz;
    dx = p1.x - v.x;
    dy = p1.y - v.y;
    dz = p1.z - v.z;
    float result = dx * v.x + dy * v.y + dz * v.z;
    if (result < 0)
    {
        v.x = -v.x;
        v.y = -v.y;
        v.z = -v.z;
    }

    for (size_t i = 0; i < 1000; i++)
    {
        p2.x = p1.x + i * 0.01 * v.x;
        p2.y = p1.y + i * 0.01 * v.y;
        p2.z = p1.z + i * 0.01 * v.z;
        inputCloudPtr->points.push_back(p2);
    }
}

/**
 * @brief 画出圆柱的轴线
 * @param const pcl::PointCloud<pcl::PointXYZ>::Ptr& inputCloudPtr        输入点云
 * @param const pcl::ModelCoefficients::ptr& coePtr                       圆柱的参数
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::axialDrawing(Pcloud& InputCloud, const pcl::ModelCoefficients::Ptr& coePtr)
{
    Point p1, p2;
    Point v;
    p1.x = coePtr->values[0] * 1000.0;
    p1.y = coePtr->values[1] * 1000.0;
    p1.z = coePtr->values[2] * 1000.0;

    v.x = coePtr->values[3];
    v.y = coePtr->values[4];
    v.z = coePtr->values[5];

    float dx, dy, dz;
    dx = p1.x - v.x;
    dy = p1.y - v.y;
    dz = p1.z - v.z;
    float result = dx * v.x + dy * v.y + dz * v.z;
    if (result < 0)
    {
        v.x = -v.x;
        v.y = -v.y;
        v.z = -v.z;
    }

    for (size_t i = 0; i < 1000; i++)
    {
        p2.x = p1.x + i * 0.01 * v.x;
        p2.y = p1.y + i * 0.01 * v.y;
        p2.z = p1.z + i * 0.01 * v.z;
        InputCloud->points.push_back(p2);
    }
}

/**
 * @brief 条件欧式聚类函数                                              
 * @param const pcl::PointCloud<pcl::PointXYZINormal>::Ptr& inputCloudPtr       输入点云 
 * @param pcl::IndicesClusters& indices                                         聚类所得的点云的索引
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::conditionalEucluster(const pcl::PointCloud<PointTypeFull>::Ptr& inputCloudPtr, pcl::IndicesClusters& indices)
{
    pcl::IndicesClustersPtr small_indices(new pcl::IndicesClusters()), large_indices(new pcl::IndicesClusters());
    pcl::ConditionalEuclideanClustering<PointTypeFull> cec(true);
    cec.setInputCloud(inputCloudPtr);
    cec.setConditionFunction(&intensityNormalSimilarity);
    cec.setClusterTolerance(0.15);
    cec.setMinClusterSize(20);
    cec.setMaxClusterSize(inputCloudPtr->size());
    cec.segment(indices);
    cec.getRemovedClusters(small_indices, large_indices);
}

/**
 * @brief 多帧点云融合函数
 * @param const pcl::PointCloud<pcl::PointXYZ>::Ptr& inputCloudPtr      输入点云
 * @param pcl::PointCloud<pcl::PointXYZ>::Ptr& outputCloudPtr           融合的输出点云
 * @param const Eigen::Matrix4f& RoT                                    融合的齐次矩阵
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::fusion(const Pcloud& inputCloudPtr, Pcloud& outputCloudPtr, const Eigen::Matrix4f& RoT)
{
    for (size_t i = 0; i < inputCloudPtr->size(); i++)
    {
        Eigen::Vector4f Otemp(0., 0., 0., 1), Atemp(0., 0., 0., 1);
        Otemp.x() = inputCloudPtr->points[i].x;
        Otemp.y() = inputCloudPtr->points[i].y;
        Otemp.z() = inputCloudPtr->points[i].z;

        Atemp = RoT * Otemp;
        outputCloudPtr->points.push_back(Point(Atemp.x(), Atemp.y(), Atemp.z()));
    }
}

/**
 * @brief 多帧点云融合函数
 * @param const pcl::PointCloud<pcl::PointXYZI>::Ptr& inputCloudPtr     输入点云
 * @param pcl::PointCloud<pcl::PointXYZI>::Ptr& outputCloudPtr          融合的输出点云
 * @param const Eigen::Matrix4f& RoT                                    融合的齐次矩阵
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::fusion(const PcloudI& inputCloudPtr, PcloudI& outputCloudPtr, const Eigen::Matrix4f& RoT)
{
    for (size_t i = 0; i < inputCloudPtr->size(); i++)
    {
        Eigen::Vector4f Otemp(0., 0., 0., 1), Atemp(0., 0., 0., 1);
        Otemp.x() = inputCloudPtr->points[i].x;
        Otemp.y() = inputCloudPtr->points[i].y;
        Otemp.z() = inputCloudPtr->points[i].z;

        Atemp = RoT * Otemp;
        // std::cout << Otemp << std::endl;
        // std::cout << Atemp << std::endl;
        PointI temp;
        temp.x = Atemp.x();
        temp.y = Atemp.y();
        temp.z = Atemp.z();
        temp.intensity = inputCloudPtr->points[i].intensity;
        outputCloudPtr->points.push_back(temp);
    }
}

/**
 * @brief 多帧点云融合函数输入点云
 * @param const pcl::PointCloud<pcl::PointXYZI>& inputCloud         输入点云
 * @param pcl::PointCloud<pcl::PointXYZI>::Ptr& outputCloudPtr      融合的输出点云
 * @param const Eigen::Matrix4f& RoT                                融合的齐次矩阵
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::fusion(const CloudI& inputCloud, PcloudI& outputCloudPtr, const Eigen::Matrix4f& RoT)
{
    for (size_t i = 0; i < inputCloud.size(); i++)
    {
        Eigen::Vector4f Otemp(0., 0., 0., 1), Atemp(0., 0., 0., 1);
        Otemp.x() = inputCloud.points[i].x;
        // std::cout << "x: " << Otemp.x() << std::endl;
        Otemp.y() = inputCloud.points[i].y;
        Otemp.z() = inputCloud.points[i].z;
        if (Otemp.x() == 0 || Otemp.y() == 0 || Otemp.z() == 0)
            continue;

        Atemp = RoT * Otemp;
        // std::cout << Otemp << std::endl;
        // std::cout << Atemp << std::endl;
        PointI temp;
        temp.x = Atemp.x();
        temp.y = Atemp.y();
        temp.z = Atemp.z();
        temp.intensity = inputCloud.points[i].intensity;
        outputCloudPtr->points.push_back(temp);
    }
}


/**
 * @brief 多帧点云融合函数。直接将点云转入深度图
 * @param const pcl::PointCloud<pcl::PointXYZI>& inputCloud         输入点云
 * @param cv::Mat& outputDepthImg                                   输出深度图
 * @param const Eigen::Matrix4f& cam2Lidar_RoT                      相机与雷达的位姿转换矩阵    
 * @param const Eigen::Matrix3f& camIntrix                          相机内参
 * @param const Eigen::Matrix4f& RoT                                融合的齐次矩阵
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::fusion(const CloudI& inputCloud, cv::Mat& outputDepthImg, const Eigen::Matrix4f& cam2Lidar_RoT, const Eigen::Matrix3f& camIntrix,
                  const Eigen::Matrix4f& RoT)
{
    Eigen::Vector4f CamCoor;
    Eigen::Vector3f PxCoor;
    float u, v;
    for (size_t i = 0; i < inputCloud.size(); i++)
    {
        Eigen::Vector4f Otemp(0., 0., 0., 1), Atemp(0., 0., 0., 1);
        //转到最后一帧点云的坐标
        Otemp.x() = inputCloud.points[i].x;
        // std::cout << "x: " << Otemp.x() << std::endl;
        Otemp.y() = inputCloud.points[i].y;
        Otemp.z() = inputCloud.points[i].z;
        if (Otemp.x() == 0 || Otemp.y() == 0 || Otemp.z() == 0)
            continue;
        Atemp = RoT * Otemp;
        if (Atemp.x() > 10)
            continue;

        //转为相机坐标系下的坐标
        CamCoor = cam2Lidar_RoT * Atemp;
        PxCoor = camIntrix * Eigen::Vector3f(CamCoor.x(), CamCoor.y(), CamCoor.z());

        //转为像素坐标
        u = PxCoor(0) / CamCoor(2);
        v = PxCoor(1) / CamCoor(2);
        if (u < 0 || u > 1280 || v < 0 || v > 1024)
            continue;
        // std::cout << "depth is: " <<  CamCoor(2) * 1000;
        outputDepthImg.at<short>(v, u) = CamCoor(2) * 1000;
    }
}

/**
 * @brief 获取旋转矩阵和平移矩阵的齐次矩阵
 * @param const Eigen::Matrix4f& inputRoT        
 * @param const Eigen::Vector4f& inputTran 
 * @return Eigen::Matrix4f 
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
Eigen::Matrix4f PCLF::RoT(const Eigen::Matrix4f& inputRoT, const Eigen::Vector4f& inputTran)
{
    Eigen::Vector4f tempTran(0, 0, 0, 0);
    tempTran = inputRoT * inputTran - inputTran;
    Eigen::Matrix4f tempRoT;
    tempRoT = inputRoT;
    tempRoT(0, 3) += tempTran.x();
    tempRoT(1, 3) += tempTran.y();
    tempRoT(2, 3) += tempTran.z();
    return tempRoT;
}

/**
 * @brief 获取原始位姿与目的位姿之间的转换矩阵
 * @param const Gydata inputGydata_Obj      原始位姿的陀螺仪数据
 * @param const Gydata inputGydata_Des      目的位姿的陀螺仪数据
 * @param Eigen::Matrix4f& RoT              原始位姿和目的位姿的转换矩阵
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
void PCLF::Gydata2RoT(const Gydata inputGydata_Obj, const Gydata inputGydata_Des, Eigen::Matrix4f& RoT)
{
    float yaw2 = -(inputGydata_Des.yaw - inputGydata_Obj.yaw) / 180 * CV_PI;
    // float yaw2 = 0;
    Eigen::Vector3f Tran2 = inputGydata_Des.Tran - inputGydata_Obj.Tran;
    RoT << cosf(yaw2), -sinf(yaw2), 0, Tran2.y(), sinf(yaw2), cosf(yaw2), 0, -Tran2.x(), 0, 0, 1, Tran2.z(), 0, 0, 0, 1;
}

/**
 * @brief 根据识别到的Lagori的半径来计算其对应的id
 * @param const float radiu     输入半径 
 * @return int                  Lagori的id
 * @author Milo 
 *  -QQ:2601171627; Email:milo_name@outlook.com
 */
int PCLF::WhichId(const float radiu)
{
    float LS = radiu / 200.;
    float Del = 5;
    int id;
    for (int i = 1; i < 6; i++)
    {
        float Ref;
        switch (i)
        {
        case 1: {
            Ref = 1;
            break;
        }
        case 2: {
            Ref = 1.375;
            break;
        }
        case 3: {
            Ref = 1.75;
            break;
        }
        case 4: {
            Ref = 2.125;
            break;
        }
        case 5: {
            Ref = 2.5;
            break;
        }
        };
        float temp = abs(LS - Ref);
        if (temp < Del)
        {
            Del = temp;
            id = i;
        }
    }
    return id;
}